#include "LBFGSBopt.h"

//vector<double>* LBFGSBopt::g_pDose = NULL;
OptData* LBFGSBopt::g_optData = NULL;
vector<SConstraint>* LBFGSBopt::g_pConstraints = NULL;
vector<double>* LBFGSBopt::g_objFunValue=NULL;

LBFGSBopt* LBFGSBopt::Instance()
{

	static LBFGSBopt instance;
	return &instance;
}

LBFGSBopt::LBFGSBopt(void)
{
}


LBFGSBopt::~LBFGSBopt(void)
{
}

void LBFGSBopt::optimization(vector<double> *resutlWeight,OptData* optData, vector<SConstraint> *pConstrain, vector<double> *objFunValue,int maxIter, double LBound){


	LBFGSBopt::g_optData = optData;
	LBFGSBopt::g_pConstraints = pConstrain;
	LBFGSBopt::g_objFunValue = objFunValue;

	this->nmax = optData->getWeightTotal()->size();
	this->mmax = 17;
	this->charLength = 60;

	char* task = new char[this->charLength];
	char* csave = new char[this->charLength];

	long int lsave[4];
	long int n, m, iprint, isave[44];
	double f, factr, pgtol, dsave[29];

	long int* nbd = new long int[nmax];
	long int* iwa = new long int[3 * nmax];
	double* x = new double[nmax];
	double* l = new double[nmax];
	double* u = new double[nmax];
	double* g = new double[nmax];
	double* wa = new double[2 * mmax * nmax + 4 * nmax + 12 * mmax * mmax + 12 * mmax];

	iprint = -1;

	//c     We specify the tolerances in the stopping criteria.
	factr = 0;//1.e+1;
	pgtol = 1.0e-15;//1.0e-9;
	n = this->nmax;
	m = this->mmax;

	//c     We now provide nbd which defines the bounds on the variables:
	//c                    l   specifies the lower bounds,
	//c                    u   specifies the upper bounds. 

	//c     First set bounds on variables. 
	//c		nbd(i)=0 if x(i) is unbounded,
	//c              1 if x(i) has only a lower bound,
	//c              2 if x(i) has both lower and upper bounds, 
	//c              3 if x(i) has only an upper bound.

	for (int i = 0; i < n; i++) {
		nbd[i] = 1;
		l[i] = LBound;
		u[i] = 1000.0;
	}

	//c     We now define the starting point.
	for (int i = 0; i < n; i++){
		x[i] = (*optData->getWeightTotal())[i];
	}

	//c     We start the iteration by initializing task.
	// (**MUST clear remaining chars in task with spaces (else crash)!**)
	strcpy(task, "START");
	for (int i = 5; i < this->charLength; i++){
		task[i] = ' ';
	}
		
	//c     This is the call to the L-BFGS-B code.
	// (* call the L-BFGS-B routine with task='START' once before loop *)
	setulb_(&n, &m, x, l, u, nbd, &f, g, &factr, &pgtol, wa, iwa, task, &iprint,
		csave, lsave, isave, dsave, (long int) 60, (long int) 60);

	// (* while routine returns "FG" or "NEW_X" in task, keep calling it *)
	while (strncmp(task, "FG", 2) == 0 || strncmp(task, "NEW_X", 5) == 0) {

		if (strncmp(task, "FG", 2) == 0) {
			//c   the minimization routine has returned to request the
			//c   function f and gradient g values at the current x

			//c        Compute function value f for the sample problem.
			f = 0;
			for(int i=0; i!=n;i++)
			{
				g[i] = 0;
			}
			//step2 = clock();
			vector<double> *tempWeight = new vector<double>(n);
			for(int indexResult = 0; indexResult!=n;indexResult++)
			{
				(*tempWeight)[indexResult] = x[indexResult];
			}
			//step3=clock();
			LBFGSBopt::g_pDose = new vector<double>;
			*LBFGSBopt::g_pDose = LBFGSBopt::g_optData->calculateDose(tempWeight);
			//step4=clock();
			vector<SConstraint>::iterator iterConstraint;
			for(iterConstraint = LBFGSBopt::g_pConstraints->begin();iterConstraint != LBFGSBopt::g_pConstraints->end();iterConstraint++)
			{
				//step5 = clock();
				//initialization
				string strStructureName = iterConstraint->strStructureName;
				double aimDose = iterConstraint->aimDose;
				double aimWeight = iterConstraint->aimWeight;
				string constraintType = iterConstraint->constraintType;
				vector<vector<double> > *vvDij = LBFGSBopt::g_optData->getDijTotal();

				vector<int> *aimStructure=NULL;
				for(int indexStr = 0; indexStr != LBFGSBopt::g_optData->getStructures()->size(); indexStr++)
				{
					if((*LBFGSBopt::g_optData->getStructures())[indexStr].getStructName() == strStructureName)
					{
						aimStructure = (*LBFGSBopt::g_optData->getStructures())[indexStr].getPoints();
					}
				}
				if(aimStructure==NULL)
				{
					cerr<<"structure error"<<endl;
				}
				//step6 = clock();
				//create func and grad[]
				double tempTotleFunc = 0;
				for(int indexPoint = 0; indexPoint!=aimStructure->size();indexPoint++)
				{
					double temp = 0;
					double contri = 0;
					double wei =0;
					unsigned int position = (*aimStructure)[indexPoint];

					temp = (*g_pDose)[position];

					if((constraintType == "upper")&&(temp>aimDose)){
						tempTotleFunc += (temp - aimDose)*(temp - aimDose);
					}else if((constraintType == "lower")&&(temp<aimDose)){
						tempTotleFunc += (temp - aimDose)*(temp - aimDose);
					}else if((constraintType == "uniform")){
						tempTotleFunc += (temp - aimDose)*(temp - aimDose);
					}
				} 
				f += (tempTotleFunc/double(aimStructure->size()))*aimWeight;

				//step7=clock();
				//calculate grad[i]
				for(unsigned int j=0;j!=n;j++)
				{
					double temp = 0;
					double temp2 = 0;
					double tempTotalGrad = 0;

					for(int indexPoint = 0; indexPoint!=aimStructure->size();indexPoint++)
					{
						temp = 0;
						temp2 = 0;
						double contri = 0;
						double wei =0;
						unsigned int position =(*aimStructure)[indexPoint];

						if((*vvDij)[position].size()>j){
							contri = (*vvDij)[position][j];
						}

						if(contri > 1e-20)
						{
							temp2 =  contri;

							temp = (*g_pDose)[position];

							if((constraintType == "upper")&&(temp>aimDose)){
								tempTotalGrad += 2*(temp - aimDose)*temp2;
							}else if((constraintType == "lower")&&(temp<aimDose)){
								tempTotalGrad += 2*(temp - aimDose)*temp2;
							}else if((constraintType == "uniform")){
								tempTotalGrad += 2*(temp - aimDose)*temp2;
							}
						}
					}
					g[j] += (tempTotalGrad/double(aimStructure->size()))*aimWeight;

				}
				//step8=clock();
			}

			cout<<f<<endl;
			g_objFunValue->push_back(f);
			//cout<<step2-step1<<"\t"<<step3-step2<<"\t"<<step4-step3<<"\t"<<step5-step4<<"\t"<<step6-step5<<"\t"<<step7-step6<<"\t"<<step8-step7<<endl;
			delete LBFGSBopt::g_pDose;
			
		} else {
			// the minimization routine has returned with a new iterate,
			// and we have opted to continue the iteration (do nothing here)
		}

		//c          go back to the minimization routine.
		setulb_(&n, &m, x, l, u, nbd, &f, g, &factr, &pgtol, wa, iwa, task, &iprint,
			csave, lsave, isave, dsave, (long int) 60, (long int) 60);

	}

	for(int i=0;i<n;i++)
	{
		resutlWeight->push_back(x[i]);		
	} 

	delete task;
	delete csave;
	delete nbd;
	delete iwa;
	delete x;
	delete l;
	delete u;
	delete g;
	delete wa;
}